#include "slam_robot/serial_bridge.hpp"
#include <rclcpp/logger.hpp>
#include <rclcpp/utilities.hpp>
#include <cstring>
#include <sys/ioctl.h>
#include <cerrno>
#include <system_error>
#include <cmath>
#include <memory>
#include <iomanip>
#include <sstream>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <array>

// 波特率映射表
static speed_t get_baud_rate(int baudrate) {
    switch(baudrate) {
        case 9600:   return B9600;
        case 19200:  return B19200;
        case 38400:  return B38400;
        case 57600:  return B57600;
        case 115200: return B115200;
        case 230400: return B230400;
        case 460800: return B460800;
        case 921600: return B921600;
        default:     return B115200;
    }
}

SerialBridge::SerialBridge(const std::string& port, int baudrate)
    : rclcpp::Node("serial_bridge"),
      port_(port),
      baudrate_(baudrate),
      frame_state_(FrameState::IDLE),
      command_(0),
      length_(0),
      data_index_(0),
      checksum_(0)
{
    RCLCPP_INFO(this->get_logger(), "Initializing serial bridge on port: %s @ %d baud", 
               port.c_str(), baudrate);
    
    // 打开串口 (POSIX标准API)
    serial_fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
    
    if (serial_fd_ < 0) {
        RCLCPP_FATAL(this->get_logger(), "Failed to open serial port: %s. Error: %s", 
                    port_.c_str(), strerror(errno));
        throw std::system_error(errno, std::generic_category(), "Failed to open serial port");
    }
    
    // 配置串口参数
    termios options{};
    if (tcgetattr(serial_fd_, &options) != 0) {
        RCLCPP_FATAL(this->get_logger(), "Failed to get serial attributes: %s", strerror(errno));
        close(serial_fd_);
        throw std::system_error(errno, std::generic_category(), "Failed to get serial attributes");
    }
    
    // 清除所有标志位
    cfmakeraw(&options);

    // 设置波特率
    speed_t baud = get_baud_rate(baudrate_);
    cfsetispeed(&options, baud);
    cfsetospeed(&options, baud);
    
    // 8位数据位，无奇偶校验，1位停止位
    options.c_cflag &= ~PARENB;     // 无奇偶校验
    options.c_cflag &= ~CSTOPB;     // 1位停止位
    options.c_cflag &= ~CSIZE;      // 清除数据位设置
    options.c_cflag |= CS8;         // 8位数据位
    
    // 禁用硬件/软件流控
    options.c_cflag &= ~CRTSCTS;    // 硬件流控
    options.c_iflag &= ~(IXON | IXOFF | IXANY); // 软件流控
    
    // 原始模式输入
    options.c_lflag &= 0;
    
    // 原始模式输出
    options.c_oflag &= 0;      // 禁用输出处理
    
    // 设置超时和最小读取字符
    options.c_cc[VMIN] = 0;         // 非阻塞模式，立即返回
    options.c_cc[VTIME] = 1;        // 100ms超时 (单位是0.1秒)
    
    // 应用配置
    if (tcsetattr(serial_fd_, TCSANOW, &options) != 0) {
        RCLCPP_FATAL(this->get_logger(), "Failed to configure serial port: %s", strerror(errno));
        close(serial_fd_);
        throw std::system_error(errno, std::generic_category(), "Failed to configure serial port");
    }
    
    // 清空串口缓冲区
    tcflush(serial_fd_, TCIOFLUSH);
    
    // 创建定时器，每10ms调用一次数据解析
    parse_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(10),
        std::bind(&SerialBridge::parseData, this)
    );
    
    // 初始化数据结构
    speed_data_ = {0, 0, 0};
    imu_data_ = {0, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    
    RCLCPP_INFO(this->get_logger(), "Serial bridge initialized successfully");
}

SerialBridge::~SerialBridge() {
    if (serial_fd_ >= 0) {
        close(serial_fd_);  // 使用POSIX close
        RCLCPP_INFO(this->get_logger(), "Serial port closed");
    }
}

void SerialBridge::parseData() {
    // 非阻塞读取串口数据
    uint8_t buffer[256];
    ssize_t bytes_read = read(serial_fd_, buffer, sizeof(buffer));
    
    if (bytes_read < 0) {
        if (errno != EAGAIN && errno != EWOULDBLOCK) {
            RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", strerror(errno));
        }
        else{
            // 添加无数据时的调试信息
            RCLCPP_DEBUG_ONCE(this->get_logger(), "No data available (EAGAIN)");
        }
        return;
    }
    
    if (bytes_read == 0) {
        RCLCPP_DEBUG_ONCE(this->get_logger(), "No data available on serial port");
        // 无数据可用
        return;
    }
    
    // 调试日志（仅在DEBUG级别启用）
    // RCLCPP_DEBUG(this->get_logger(), "Received %zd bytes", bytes_read);
    // 添加原始数据十六进制输出
    RCLCPP_DEBUG(this->get_logger(), "Received %zd bytes", bytes_read);
    // 添加详细的原始数据日志
    std::stringstream hex_data;
    for (int i = 0; i < std::min(static_cast<ssize_t>(16), bytes_read); i++) {
        hex_data << "0x" << std::hex << std::setw(2) << std::setfill('0') 
                 << static_cast<int>(buffer[i]) << " ";
    }
    RCLCPP_DEBUG(this->get_logger(), "Data: %s", hex_data.str().c_str());
    
    // 处理接收到的每一个字节
    for (ssize_t i = 0; i < bytes_read; i++) {
        uint8_t byte = buffer[i];

        FrameState prev_state = frame_state_;  // 保存之前的状态用于日志
        
        switch (frame_state_) {
            case FrameState::IDLE:
                if (byte == FRAME_HEADER) {
                    RCLCPP_DEBUG(this->get_logger(), "Frame start detected");
                    frame_state_ = FrameState::HEADER1;
                    checksum_ = 0; // 重置校验和
                }
                break;
                
            case FrameState::HEADER1:
                if (byte == FRAME_HEADER) {
                    frame_state_ = FrameState::COMMAND;
                } else {
                    RCLCPP_DEBUG(this->get_logger(), "Invalid second header byte: 0x%02X", byte);
                    frame_state_ = FrameState::IDLE; // 无效头，重置状态机
                }
                break;
                
            case FrameState::COMMAND:
                command_ = byte;
                checksum_ = byte; // 校验和从命令码开始
                RCLCPP_DEBUG(this->get_logger(), "Command byte: 0x%02X", byte);
                frame_state_ = FrameState::LENGTH;
                break;
                
            case FrameState::LENGTH:
                length_ = byte;
                checksum_ += byte;
                data_index_ = 0;

                RCLCPP_DEBUG(this->get_logger(), "Data length: %d", length_);
                
                // 检查长度是否有效
                if (length_ > 0 && length_ < sizeof(rx_buffer_)) {
                    frame_state_ = FrameState::DATA;
                } else {
                    RCLCPP_WARN(this->get_logger(), "Invalid frame length: %d", length_);
                    frame_state_ = FrameState::IDLE;
                }
                break;
                
            case FrameState::DATA:
                if (data_index_ < length_) {
                    rx_buffer_[data_index_++] = byte;
                    checksum_ += byte;
                }
                
                if (data_index_ >= length_) {
                    frame_state_ = FrameState::CHECKSUM;
                }
                break;
                
            case FrameState::CHECKSUM:
                if (checksum_ == byte) {
                    frame_state_ = FrameState::TAIL;
                } else {
                    RCLCPP_WARN(this->get_logger(), 
                                "Checksum mismatch! Expected: 0x%02X, Received: 0x%02X", 
                                checksum_, byte);
                    frame_state_ = FrameState::IDLE;
                }
                break;
                
            case FrameState::TAIL:
                if (byte == FRAME_TAIL) {
                    // 完整帧接收成功，处理数据
                    processFrame();
                } else {
                    RCLCPP_WARN(this->get_logger(), "Missing frame tail (expected 0xBB, got 0x%02X)", byte);
                }
                frame_state_ = FrameState::IDLE;
                checksum_ = 0; // 重置校验和
                break;
                
            default:
                frame_state_ = FrameState::IDLE; // 未知状态，重置
                checksum_ = 0;
                break;
        }
        // 记录状态变化
        if (prev_state != frame_state_) {
            RCLCPP_DEBUG(this->get_logger(), "State changed: %d -> %d", 
                        static_cast<int>(prev_state), 
                        static_cast<int>(frame_state_));
        }        
    }
}

void SerialBridge::processFrame() {
    // 使用互斥锁保护共享数据
    std::lock_guard<std::mutex> lock(data_mutex_);
    
    switch (command_) {
        case CMD_IMU_DATA:
            if (length_ == 28) {  // IMU数据应为28字节
                // 解析时间戳（小端序）
                imu_data_.timestamp = 
                    (static_cast<uint32_t>(rx_buffer_[0]) << 24) |
                    (static_cast<uint32_t>(rx_buffer_[1]) << 16) |
                    (static_cast<uint32_t>(rx_buffer_[2]) << 8)  |
                    static_cast<uint32_t>(rx_buffer_[3]);
                
                // 解析加速度（小端序，float格式）
                memcpy(&imu_data_.accel_x, rx_buffer_+4, 4);
                memcpy(&imu_data_.accel_y, rx_buffer_+8, 4);
                memcpy(&imu_data_.accel_z, rx_buffer_+12, 4);
                // 解析陀螺仪（小端序，float格式）
                memcpy(&imu_data_.gyro_x, rx_buffer_+16, 4);
                memcpy(&imu_data_.gyro_y, rx_buffer_+20, 4);
                memcpy(&imu_data_.gyro_z, rx_buffer_+24, 4);
                
                // 调试日志
                RCLCPP_DEBUG(this->get_logger(), 
                            "IMU Data: ts=%u, Accel(%.2f,%.2f,%.2f) m/s², Gyro(%.2f,%.2f,%.2f) rad/s",
                            imu_data_.timestamp, 
                            imu_data_.accel_x, imu_data_.accel_y, imu_data_.accel_z,
                            imu_data_.gyro_x, imu_data_.gyro_y, imu_data_.gyro_z);
            } else {
                RCLCPP_WARN(this->get_logger(), "Invalid IMU data length: %d (expected 28)", length_);
            }
            break;
            
        case CMD_SPEED_DATA:
            if (length_ == 6) {  // 速度数据应为6字节（里程计用）
                // 解析速度（大端序: MSB在前）
                speed_data_.vx = static_cast<int16_t>((rx_buffer_[0] << 8) | rx_buffer_[1]);
                speed_data_.vy = static_cast<int16_t>((rx_buffer_[2] << 8) | rx_buffer_[3]);
                speed_data_.vz = static_cast<int16_t>((rx_buffer_[4] << 8) | rx_buffer_[5]);
                
                // 调试日志
                RCLCPP_DEBUG(this->get_logger(), 
                            "Speed Data: Vx=%d, Vy=%d, Vz=%d mm/s/rad/s", 
                            speed_data_.vx, speed_data_.vy, speed_data_.vz);
            } else {
                RCLCPP_WARN(this->get_logger(), "Invalid speed data length: %d (expected 6)", length_);
            }
            break;
            
        default:
            RCLCPP_WARN(this->get_logger(), "Unknown command: 0x%02X (Ignored)", command_);
            break;
    }
}

void SerialBridge::setSpeedCommand(int16_t vx, int16_t vy, int16_t vz) {
    std::array<uint8_t, 11> buffer{};
    
    // 1. 构建命令帧结构
    buffer[0] = FRAME_HEADER;  // 帧头1
    buffer[1] = FRAME_HEADER;  // 帧头2
    buffer[2] = TEST_CMD;      // 命令类型
    buffer[3] = 6;             // 数据长度 (3个int16_t)
    
    // 2. 填充速度数据 (大端序)
    buffer[4] = static_cast<uint8_t>(vx >> 8);   // Vx高位
    buffer[5] = static_cast<uint8_t>(vx & 0xFF); // Vx低位
    buffer[6] = static_cast<uint8_t>(vy >> 8);   // Vy高位
    buffer[7] = static_cast<uint8_t>(vy & 0xFF); // Vy低位
    buffer[8] = static_cast<uint8_t>(vz >> 8);   // Vz高位
    buffer[9] = static_cast<uint8_t>(vz & 0xFF); // Vz低位
    
    // 3. 计算校验和 (命令类型 + 长度 + 6字节数据)
    buffer[10] = buffer[2] + buffer[3];
    for (int i = 4; i <= 9; i++) {
        buffer[10] += buffer[i];
    }
    
    // 4. 发送命令
    sendCommand(buffer.data(), buffer.size());
    
    RCLCPP_DEBUG(this->get_logger(), "Sent speed command: vx=%d, vy=%d, vz=%d mm/s/rad/s", vx, vy, vz);
}

void SerialBridge::sendCommand(const uint8_t* data, size_t length) {
    if (serial_fd_ < 0) {
        RCLCPP_WARN(this->get_logger(), "Serial port not open, cannot send command");
        return;
    }
    
    ssize_t bytes_written = write(serial_fd_, data, length);
    
    if (bytes_written < 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to write to serial port: %s", strerror(errno));
    } else if (static_cast<size_t>(bytes_written) != length) {
        RCLCPP_WARN(this->get_logger(), "Partial write: %zd/%zu bytes written", bytes_written, length);
    } else {
        RCLCPP_DEBUG(this->get_logger(), "Sent %zu bytes successfully", length);
    }
}
